from matchteris.env import Assemble_Suction
from MoveBlock.move_block import BlockControl, ArmControl, posThreshold, angleThreshold
import numpy as np
from time import time

step = 1000

class Operation:
    def __init__(self, block_name:str, destination:list[int,int], spinAngle:float, 
                    distance_off:np.ndarray[float]=np.array([0,0,-2e-4]), angle_off:float=0.05):
        self.block_name = block_name
        self.destination = destination
        self.spinAngle = spinAngle
        self.distance_off = distance_off
        self.angle_off = angle_off
    def __str__(self):
        return "block_name: " + self.block_name + "\n" + \
                "destination: " + str(self.destination) + "\n" + \
                "spinAngle: " + str(self.spinAngle) + "\n" + \
                "distance_off: " + str(self.distance_off) + "\n" + \
                "angle_off: " + str(self.angle_off) + "\n"

class Splice:
    # 方法一
    splicing_series1 = []
    splicing_series1.append(Operation("red0/", [9,2], 0))    # 1
    splicing_series1.append(Operation("red1/", [8,2], 0))    # 2
    splicing_series1.append(Operation("red2/", [7,2], 0))    # 3
    splicing_series1.append(Operation("red3/", [6,2], 0))    # 4
    splicing_series1.append(Operation("orange0/", [5,0], 0))    # 5
    splicing_series1.append(Operation("orange1/", [3,0], 0))    # 6
    splicing_series1.append(Operation("orange2/", [1,0], 0))    # 7
    splicing_series1.append(Operation("orange3/", [5,2], 0))    # 8
    splicing_series1.append(Operation("yellow0/", [3,2], 0))    # 9
    splicing_series1.append(Operation("blue0/", [1,3], 0))    # 10
    splicing_series1.append(Operation("brown0/", [3,4], 0))     # 11
    splicing_series1.append(Operation("orange4/", [9,4], 0))    # 12
    splicing_series1.append(Operation("yellow1/", [7,4], 0))    # 13
    splicing_series1.append(Operation("green0/", [4,5], 0))    # 14
    splicing_series1.append(Operation("red4/", [0,6], 0))       # 15
    splicing_series1.append(Operation("brown1/", [2,6], 0))     # 16

    splicing_series1.append(Operation("blue1/", [5,6], 90))     # 17
    splicing_series1.append(Operation("yellow2/", [9,6], 0))    # 18
    splicing_series1.append(Operation("yellow3/", [5,7], 0))     # 19
    splicing_series1.append(Operation("yellow4/", [2,8], 180))    # 20
    splicing_series1.append(Operation("purple0/", [6,8], 90))    # 21
    splicing_series1.append(Operation("green1/", [8,8], 90))    # 22
    splicing_series1.append(Operation("purple1/", [1,8], -90))    # 23
    splicing_series1.append(Operation("purple2/", [0,11], 90))    # 24
    splicing_series1.append(Operation("brown2/", [3,10], 90))     # 25
    splicing_series1.append(Operation("brown3/", [7,10], 90))     # 26
    splicing_series1.append(Operation("green2/", [9,10], 0))     # 27
    splicing_series1.append(Operation("blue2/", [6,10], 0))     # 28
    splicing_series1.append(Operation("blue3/", [3,11], 0))     # 29
    splicing_series1.append(Operation("blue4/", [5,11], 0))     # 30
    splicing_series1.append(Operation("green3/", [7,12], 0))     # 31
    splicing_series1.append(Operation("green4/", [9,12], 0))     # 32
    splicing_series1.append(Operation("purple3/", [0,12], 180))    # 33
    splicing_series1.append(Operation("purple4/", [4,13], 0))    # 34


    # 方法二
    splicing_series2 = []
    splicing_series2.append(Operation("orange0/", [9,0], 0)) # 1
    splicing_series2.append(Operation("orange1/", [9,2], 0)) # 2
    splicing_series2.append(Operation("red0/", [7,2], 0))    # 3
    splicing_series2.append(Operation("red1/", [6,2], 0))    # 4
    splicing_series2.append(Operation("yellow0/", [5,0], 0)) # 5
    splicing_series2.append(Operation("red2/", [0,2], 0))    # 6
    splicing_series2.append(Operation("yellow1/", [1,0], -90)) # 7
    splicing_series2.append(Operation("yellow2/", [2,3], 90)) # 8
    splicing_series2.append(Operation("orange2/", [4,1], 0)) # 9
    splicing_series2.append(Operation("green0/", [5,3], 0))  # 10
    splicing_series2.append(Operation("green1/", [3,4], 0))  # 11
    splicing_series2.append(Operation("green2/", [3,6], 0))  # 12
    splicing_series2.append(Operation("green3/", [5,5], 0))  # 13
    splicing_series2.append(Operation("orange3/", [9,4], 0)) # 14
    splicing_series2.append(Operation("orange4/", [7,4], 0)) # 15
    splicing_series2.append(Operation("red3/", [0,6], 0))    # 16
    splicing_series2.append(Operation("red4/", [1,6], 0))    # 17
    splicing_series2.append(Operation("purple0/", [5,7], 0)) # 18
    splicing_series2.append(Operation("brown0/", [8,7], 90))  # 19
    splicing_series2.append(Operation("blue0/", [7,7], 0))   # 20
    splicing_series2.append(Operation("brown1/", [8,8], 180))  # 21
    splicing_series2.append(Operation("brown2/", [1,9], 90))  # 22
    splicing_series2.append(Operation("blue1/", [3,9], 90))   # 23
    splicing_series2.append(Operation("green4/", [5,9], 0))  # 24
    splicing_series2.append(Operation("blue2/", [7,9], 0))   # 25
    splicing_series2.append(Operation("blue3/", [9,10], 0))   # 26
    splicing_series2.append(Operation("blue4/", [1,10], 0))   # 27
    splicing_series2.append(Operation("yellow3/", [2,12], 90)) # 28
    splicing_series2.append(Operation("brown3/", [4,11], 0))  # 29
    splicing_series2.append(Operation("yellow4/", [6,11], 180)) # 30
    splicing_series2.append(Operation("purple1/", [5,12], 90)) # 31
    splicing_series2.append(Operation("purple2/", [7,13], 0)) # 32
    splicing_series2.append(Operation("purple3/", [4,13], 0)) # 33
    splicing_series2.append(Operation("purple4/", [0,13], 90)) # 34

    @classmethod
    def Splice(cls, env, operation:Operation):
        print("正在拼接方块：" + operation.block_name + ", 目标位置：" + str(operation.destination) + ", 旋转角度：" + str(operation.spinAngle))
        BlockControl.MoveBlock(env, operation.block_name, operation.destination[0], operation.destination[1], 
                               operation.spinAngle, posThreshold, angleThreshold, step,
                               operation.distance_off, operation.angle_off)
        print("拼接完成")

    @classmethod
    def SpliceSeries(cls, env, operation_series:list[Operation]):
        start_time = time()
        print("正在拼接序列")
        for operation in operation_series:
            cls.Splice(env, operation)
        end_time = time()
        print("拼接完成，用时：" + str(end_time - start_time) + "s")


if __name__ == "__main__":
    env = Assemble_Suction()
    BlockControl.InitEnv(env)
    print("使用方法一拼接序列")
    Splice.SpliceSeries(env, Splice.splicing_series1)
    #BlockControl.InitEnv(env)
    #print("使用方法二拼接序列")
    #Splice.SpliceSeries(env, Splice.splicing_series2)